import os
import yaml
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
from moveit_configs_utils import MoveItConfigsBuilder
from launch.substitutions import LaunchConfiguration

def generate_launch_description():
    # --- 1. 定义启动参数 ---
    # use_sim 参数，用于区分是使用仿真时间还是真实时间
    use_sim_arg = DeclareLaunchArgument(
        'use_sim',
        default_value='false',
        description='Use simulation (Gazebo) or real hardware')
    
    use_sim_time = LaunchConfiguration('use_sim')

    # ===================================================================
    # --- 2. 加载机械臂所需的 MoveIt 和 Servo 配置 ---
    #    (这部分直接从机械臂的launch文件继承，对于机械臂控制是必需的)
    # ===================================================================
    
    # 加载 MoveIt 配置
    moveit_config = (
        MoveItConfigsBuilder("so101", package_name="lerobot_moveit")
        .to_moveit_configs()
    )

    # 加载 Servo 的 YAML 配置文件
    servo_yaml_path = os.path.join(
        get_package_share_directory('joystick_alphadog_with_so101_servo'), 
        'config', 
        'so101_servo_config.yaml'
    )
    with open(servo_yaml_path, 'r') as file:
        servo_yaml = yaml.safe_load(file)
    servo_params = {"moveit_servo": servo_yaml}

    # ===================================================================
    # --- 3. 定义所有需要启动的节点 ---
    # ===================================================================

    # --- 节点 1: joy_node ---
    # 负责读取手柄的物理输入并发布 /joy 话题
    joy_node = Node(
        package="joy", 
        executable="joy_node", 
        name="joy_node", 
        parameters=[{'use_sim_time': use_sim_time}],
    )

    # --- 节点 2: servo_node ---
    # MoveIt Servo 的核心节点，用于机械臂的笛卡尔空间控制
    servo_node = Node(
        package="moveit_servo",
        executable="servo_node_main",
        parameters=[
            servo_params,
            moveit_config.robot_description,
            moveit_config.robot_description_semantic,
            moveit_config.robot_description_kinematics,
            {'use_sim_time': use_sim_time},
        ],
        output="screen",
    )
    
    # --- 节点 3: 统一控制器节点 (关键替换) ---
    # 这是我们刚才整合的那个Python脚本的节点
    # !! 注意 !!: 你需要将 'your_package_name' 和 'unified_joy_controller' 
    #             替换成你实际的功能包名和可执行文件名
    unified_controller_node = Node(
        package='joystick_alphadog_with_so101_servo',  # <-- TODO: 修改为你的功能包名
        executable='unified_joy_controller', # <-- TODO: 修改为你在setup.py中定义的可执行文件名
        name='unified_joystick_controller_node',
        output='screen',
        parameters=[{'use_sim_time': use_sim_time}],
        remappings=[("delta_twist_cmds", "/servo_node/delta_twist_cmds")]
    )

    # ===================================================================
    # --- 4. 返回 LaunchDescription 对象 ---
    # ===================================================================
    # 包含了要启动的所有参数和节点
    return LaunchDescription([
        use_sim_arg,
        joy_node,
        servo_node,
        unified_controller_node,
    ])